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Motion Planning of Nonholonomic Systems 
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Abstract 



Nonholonomic mechanical systems are governed by constraints of motion that art nomntegrable 
differential expressions. Unlike holonornic constraints, these differential constraints do not reduce the 
number of dimensions of the configuration space of a system Therefore a nonholonomic system can 
access a configuration space of dimension higher than the number of the degrees of freedom of the 
system. In this paper, we develop an algorithm for planning admissible trajectories for nonholonomic 
systems that will take the system from one point in its configuration space to another. In our algorithm 
we first converge the independent variables to their desired values and then use closed trajectories of the 
independent variable to converge the dependent variables. We use Stokes r s theorem in our algorithm 
to convert the problem of finding a closed path into that of finding a surface area in the space of 
the independent variables, such that the dependent variables converge to their desired values as the 
independent variables traverse along the boundary of this surface area. The use of Stokes’s theorem 
simplifies the motion planning problem and also imparts global characteristics. The salient features of 
our algorithm are apparent in the two examples we discuss - a planar space robot and a disk rolling 
without slipping on a flat surface. 



t Assistant Professor 
t Graduate Student 



1 



1. Introduction 



Noriholonornic rneclianical systems are governed by constraints of motion that are 
nonintegrable differential expressions of the form 

n 

T. Qj, dq, + a Jt dt = 0, j = 1 , 2, • • • to (1) 

t=l 

where, the g’s represent the generalized coordinates, t represents time, and the a’s are, 
in general functions of the q’s and t. As a result of the nonintegrable nature of these 
differential constraints, it is not possible to obtain functions of tfie form 

l, 9-2, •••.<?». 0 = °, 3 = 1,2, • • • m (2) 

that will enable us to eliminate some of the dependent variables. Naturally, noriholonornic 
systems require more coordinates for their description than there are degrees of freedom 
in the system. 

An interesting feature of noriholonornic mechanical systems is their ability to access 
a configuration space of dimension higher than the number of it’s degrees of freedom. A 
simple example is that of a disk rolling without slipping on a flat surface. The configu- 
ration space of the disk rolling on the x-y plane, shown in Fig. 1 , is described by the four 
coordinates (x,y,0,a), but the degrees of freedom of the system is only two because of the 
following two nonholonornic constraints 



(lx —r sin a dO = 0 

dy — r cos adO = 0 (3) 

Inspite of having only two degrees of freedom, it is quite intuitive that the rolling disk can 
arrive at any configuration ( x,y,0,a ) from any other, through proper path planning. Such 
a property is common to noriholonornic mechanical systems and can be attributed to the 
nonintegrable nature of tbeir differential constraints. 

For the rolling disk, our intuition can be strengthened if we consider the following 
example. Suppose, it is desired that the disk in Fig.l change its coordinates from (x, y,0,a) 
to ( x d ,y d ,0,a ). Then a feasible trajectory would be the path segments AO and OC. The 
disk would roll forward from A to O, and then roll backward from O to C. The individual 
path segments AO and OC should have equal lengths such that 0 comes back to its initial 
value at the end of the path. Furthermore, the straigfit lines AD and CD should be tangent 
to the path segments AO and OC respectively, at the points A and C. This will ensure that 
the net change of the variable a will also be zero over the complete path. Such a path can 
always be planned and this leads us to believe that the dependent variables x and y can 
indeed be changed arbitrarily through cyclic motion of the independent variables 0 and a. 
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Therefore lo converge all the configuration variables of the disk from one set of values to 
another, we could first converge the independent variables from their initial values to their 
desired values without being concerned about the evolution of the dependent variables, and 
then use cyclic motion of the independent variables to converge the dependent variables 
to their desired values. 

In this paper we will develop an algorithm for nonholonomic motion planning - one 
that will enable us to converge all the configuration variables of a nonholonomic system 
from one set of values to another. This algorithm will follow a two step procedure - first 
converge the independent variables, and then use cyclic motion of the independent variables 
to converge the dependent variables. 

The nonholonomic motion planning problem has been the focus of attention of various 
researchers in the recent past. Specifically, researchers have looked into the problems of 
mobile robot navigation [1], [5], [6], [10], parking a front-wheel drive car or a car with 
multiple trailers [9], [10], dextrous manipulation with robotic fingers [2], attitude control 
of a satellite using two rotors instead of gas-jets, reconfiguration of a space manipulator 
or a space structure using only internal motion [12], etc. The multibody car system was 
studied in [10] and it was concluded that it is a well controllable system. This result was 
obtained by first constructing the control Lie algebra. The controllability was concluded 
by showing that the rank of the control Lie algebra is equal to the dimension of the state 
space, at every point in the state space. Such an analysis only provides sufficient conditions 
for the controllability and is useful for simple nonholonomic systems. It cannot be used 
to verify the controllability of a complex system like a 6-DOF space robot. Assuming 
the existence of a feasible trajectory connecting an initial and some desired values of the 
generalized coordinates, the nonholonomic motion planning of space robots was discussed 
in [12]. The nonholonomy of a space robot is attributed to the conservation of its angular 
momentum. A space robot consisting of a six joint manipulator mounted on a space 
vehicle was described by nine generalized coordinates consisting of six joint angles of the 
manipulator and the three Euler angles of orientation of the space vehicle. By directly 
controlling only the joints of the manipulator, it was shown that it is possible to converge 
all the nine state variables to their desired values. The trajectory was planned using a 
Lyapunov function and by adopting a bi-directional approach. 

An algorithm for steering a general class of nonholonomic systems was developed in 
[11] using sinusoids. This algorithm was applied for the motion planning of a front wheel 
drive cart, and reconfiguration of a hopping robot in flight phase. In this algorithm, the 
independent variables were first steered to their desired configuration ignoring the evolution 
of the dependent variables. Consequently, the dependent variables were converged to their 
desired values using closed trajectories of the independent variables. Such an approach 
was proposed earlier [15], for the motion planning of a space manipulator, where the cyclic 
motion of the joints of the space manipulator was used Lo change the orientation of the 
whole system. 
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In this paper, we will discuss the motion planning of nonholonornic systems using 
an algorithm in which the prerogative is to find a closed trajectory of the independent 
variables that converge the dependent variables to their desired values. In our approach, 
we use Stokes’s theorem to reduce this problem into finding a surface area such that the 
dependent variables converge to their desired values while the independent variables travel 
along the boundary of this surface area. The main advantage is the global nature of our 
planning algorithm, unlike the local path planning approach based on Lyapunov functions 
[12]. Due to the global nature of the algorithm, questions pertaining to the reachability of 
the system can be readily answered, problems related to singularity can be tackled, and 
feasible trajectories can be easily planned even in the presence of additional constraints. 
For a nonholonornic system like a space robot, these additional constraints may appear in I 
the form of joint limits or obstacles in the workspace. Our algorithm additionally provides 
us with insight into trajectories that produce repeatable motion. Repeatability in the 
motion may be simply a desirable property as in the case of space robots, or may even be 
used for singularity avoidance as in the case of a rolling disk. 

This paper is organized as follows. In section 2 we discuss the mathematical prelim- 
inaries and the properties of nonholonornic systems. In section 3, we discuss some of the 
issues related to the motion planning of nonholonornic systems. In section 4, we present 
our algorithm for the nonholonornic motion planning through examples. Specifically, we 
discuss the motion planning of a planar space robot and a disk rolling without slipping 
on a flat surface. The different salient features of our algorithm are apparent in these two 
examples. 



2. Mathematical preliminaries 



2.1 Line and surface integrals: Stokes’s theorem 



In this section we recall Stokes’s [8] theorem used for the tranformation of line integrals 
into surface integrals, and vice verca. The material discussed in this section will serve as 
a mathematical tool for the trajectory planning of nonholonornic systems. 



Theorem 1: Stokes’s Theorem Let S be a piecewise smooth oriented surface* in space 
and let the boundary of S be a piecewise smooth closed curve C. Let v(x,y, z) be a 
continuous vector function which has continuous first partial derivatives in a domain in 
space which contains S. Then 



J J n 7 '(V x v)dA = ^ v t ds 



(4) 



where, n is the unit vector normal to the surface S on that side of S which is taken as the 
positive side. The positive direction along C is then defined as the direction along which 



* If a surface S has a unique normal whose direction depends continuously on the points 
of S, then S is called a smooth surface. If S is not smooth but can be subdivided into 
finitely many smooth portions, then it is called a piecewise smooth surface. 
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an observer, traveling on the positive side of S , would proceed in keeping the enclosed area 
to his left, (see Fig . 2 (a)). v t is the component of v in the direction of the tangent vector 
of C. 

If the direction cosines of the unit vector n normal to the surface S are a, /?, and 7 , 
and if v = 1^1 + v 2 j + v 2 k, then Stokes’s theorem can be written as 




dv'2 

dz 



cos a + 



(dv x 

V 



du 3 \ a 

& ) cos(j+ 



( dv2 

\d x 



dy) 



cos 7 



dA 




dx + v 2 dy + v-s dz) 



If we restrict ourselves to the x-y plane, then Stokes’s theorem simplifies to the form 



(5) 



J J ~ dxdy = / dx + V2 dy ^ 



( 6 ) 



whicli is essentially a statement of Green’s theorem (8). For the above equation the positive 
direction of travel along the closed curve C is shown in Fig. 2 (b). This directly followed 
from Eq.(5) where we substituted (a,/3,7) = ( 71 / 2 , 71 / 2 , 0 ). We may change the direction of 
the closed curve C in Eq.(O) by using (a,/?, 7) = (71/2,71/2,71) in Eq (5). This will lead to a 
change in sign of the surface integral in Eq.(6). 



Another important theorem that will serve as an important tool for our analysis deals 
with the path independence of line integrals. This theorem is formally stated next [8] 



Theorem 2: Let v = 17 i + v 2 j + k, and let v u v 2 , and be continuous functions of 

x, y, and z in a domain D of space. Then the line integral 



J (vi dx + v 2 dy + v 2 dz) (7) 

is independent of path if and only if the differential form under the integral sign in exact 
in D , or equivalently the integral is zero for every simple closed path in D, or equivalently 
Vxt) = 0, everywhere in D. 

From the above theorem we see that the necessary and sufficient condition for the 
exactness of the differential form under the integral sign in Eq.(7) is 



dv 2 du 2 dvz dv\ dv\ du 2 

dz dy ’ dx dz dy dx 



2.2 Properties of nonholonomic systems 

In this section we discuss some of the important properties of nonholonomic systems. 
These properties will aid us to develop the motion planning schemes in the section 4. 

In section 1 we mentioned that nonholonomic constraints are nonintegrable expressions 
of the form as in Eq.(l) that cannot be simplified into expressions of the form as in Eq.(2). 
To further our discussion, we consider again the example of the disk rolling without slipping 
on a flat surface whose first constraint equation is 
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dx — r sin adO = 0 (9) 

The above constraint is not an exact differential since there exists no function <p(x,a,0) 
such that Eq.(9) can be reduced to the form 

d4>=z ^ dx + ^ da+ ^ d0 = 0 

Furthermore, Eq.(9) cannot be multiplied by an integrating factor to yield an exact dif- 
ferential. Hence it is not integrable*. It can be shown that the necessary and sufficient 
condition for the integrability of the differential equation 



v i dx + V 2 dy + v$ dz = 0 



is that 



V\ 




( 10 ) 



Applying this criterion to Eq.(9) we confirm that the expression is not integrable. In the 
more general case, the necessary and sufficient condition that the differential constraint in 
n variables (Ince, 1956) 



Vi dx\ -f V2 dx 2 -f • • • + v n dx n = 0 
is integrable, is that the set of equations 



(dv h 

\dx x 





+ Vx 





(A,/i,i/= 1, 2, • • • , n) 



( 11 ) 



are satisfied simultaneously, and identically. 

The nonholonomic property of a dynamical system can also be ascertained from the 
noninvolutive property of the distribution that spans the tangent space of the system, using 
Frobenius’s theorem. If X t e R n , % - 1,2, ■■■ ,m denote the vector fields of the system, then 
the distribution A = span {X[, X 2 , ■ ■ •, is involutive if and only if A is closed under 
Lie bracket operations. Otherwise, the system is noninvolutive or nonholonomic. In the 
case of the rolling disk, 





/ ± \ 






f°\ 


/ r sin a \ 


. A 
X = 


J 

\d) 


| = a + X.1 0, 


<11 

* 


! 

\ i/ 


v A / r cos ol 
,A 2 = j 

V 0 / 



( 12 ) 



[X!,X 2 ] = Xi - X 2 = (r COSQ — r sin a 0 0) T 

* A differential expression is integrable if it is an exact differential or can be converted 
into an exact differential after multiplying with an integrating factor. 
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Clearly, Xi, X 2 , and their Lie bracket [X U X 2 ] are linearly independent. This reconfirms 
that the rolling disk is a noninvolutive or a nonholonomic system. 

The discussion in this section so far enables us to ascertain the nonholonomy of a 
dynamical system from its differential constraints. We now investigate the manifestation 
of these nonholonomic constraints. 

If a dynamical system is described by n generalized coordinates, and m holonornic 
constraints of the form as in Eq.(2), the motion of the system is always confined to a 
manifold or surface of dimension (n - m), which is equal to the number of degrees of 
freedom of the system. Then, if we specify the ( n-m ) independent variables, it is possible 
to uniquely determine the remaining m dependent variables. This is not true when the 
constraints are nonholonomic or nonintegrable expressions of the form as in Eq.(l). The 
kinematic effect of a nonholonomic constraint is to constrain the direction of the allowable 
motions at any given point in the configuration space. But this does not reduce the number 
of dimensions in the configuration space, nor does it limit the variety of configurations 
available to the system. As a direct consequence, given the values of the independent 
variables, it is not possible to uniquely specify the values of the dependent variables of a 
nonholonomic system. When the independent variables take one set of values from another, 
the change in the dependent variables depend upon the path taken by the independent 
variables. Quite naturally, if the independent variables travel along a closed path, the 
values of the dependent variables at the beginning and end of the path are usually not the 
same. 

The above mentioned property of a nonholonomic system is better understood by the 
use of Theorem 2 on line integrals. Comparing Eq.(8) (conditions for exactness) to Eq.(10) 
(conditions for integrability), or directly from the definition of integrability, we know that 
exactness implies integrability*. Therefore it follows that a nonintegrable expression is not 
exact. Consider now a nonholonomic system where one of the dependent variables is p and 
it is constrained by the differential expression dp = v\dx + v 2 dy + v 2 dz, where x, y, and z 
are the independent variables. v\ , v 2> and V3 are continuous functions of x, y, and z. Since 
the system is nonholonomic or nonintegrable, the differential form vj dx + v 2 dy + v 2 dz is not 
exact. Therefore it follows from Theorem 2 that the change of p is path dependent, and 
this change is not zero for every closed path. This suggests the following. 

1. It is possible to change the coordinates of the dependent variable p of the nonholonomic 
system using appropriate closed trajectories of the independent variables, and 

2. There may exist some closed paths for which the path dependent integral in Eq.(7) 
will be zero for the nonholonomic system. 

On the basis of statement 1 discussed above, we now assume that there exists some closed 

* Integrability however does not imply exactness because an integrable differential ex- 
pression could have become exact only after it was multiplied by some multiplicating 
factor. 
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trajectory C of the independent variables x , y, and z that produce a change in the dependent 
variable p by some desired amount A p. If (x 0 ,y 0 , zo) be any point on this closed trajectory 
and if the initial configuration of the system is (x 0l yo,2o,po), then after the system moves 
along C once, its configuration will be (x 0 ,yo, zo.po + A p) (refer to Fig.3 (a)). If the closed 
curve C was traversed in the opposite direction, then the final configuration of the system 
would have been (xo.j/o.^o.po- Ap). Now consider the initial configuration of the system to 
be (x', y', z',p 0 ), such that ( x',y', z') does not lie on C. Let P be any path segment connecting 
the point (x', y', z') and any point (x 0 ,yo, 20 ) on the closed curve C. Let 6p denote the change 
in the dependent variable p, as x, y, and z move along the path segment P from (x',y\ z') 
to (x 0> po,zo)- Then, if the system moves from the initial configuration ( x',y',z',po ) to the 
closed curve C along P, then moves once along the closed curve C , and finally retraces the 
path P backwards, the configuration of the system at the end of the path (see Fig.3 (b)) will 
be (x', y 1 , z / ,p 0 + Ap). This is true because the surface integral of the closed curve beginning 
and ending at the point (x', y\z') is equal to the surface integral of the closed curve C. 
From this discussion it follows that the closed curve C that can bring about the desired 
change in the dependent variable can lie anywhere in the space defined by the independent 
generalized coordinates - it does not have to pass through the initial configuration of the 
system. Of course, it would be simpler to plan a closed path passing through the initial 
configuration of the system but then such a path may not be feasible due to singularity 
problems. We will discuss the singularity problem in the particular situation of a rolling 
disk, in section 4. 

In regards to statement 2 discussed above, we would just like to mention that closed 
trajectories of the independent variables that result in closed trajectories of the dependent 
variables (repeatable motion) will be of importance to us in the context of nonholonomic 
motion planning. Repeatability in the motion may be simply a desirable property as in 
the case of space robots, or may even be used for singularity avoidance as in the case of a 
rolling disk. In section 4 we will investigate into the repeatability of the motion of a space 
robot and of a rolling disk. 

3. Issues related to Nonholonomic Motion Planning 

The configuration space of a nonholonomic system is described by the set of its in- 
dependent and the dependent variables. The task of nonholonomic motion planning is 
to generate trajectories of the independent variables that will take the system from its 
current configuration to some desired configuration. In the context of the rolling disk, the 
motion planning would therefore refer to the generation of the 6 and a trajectories that 
will take the system from some initial configuration {x,,y t> 0 t , a,) to some final configuration 

The question that naturally arises in the context of motion planning is related to the 
reachability* of the system. In the case of the rolling disk, we know that any configuration 
of the system is reachable from any other. This follows from our discussion in section 1. For 

* A number of researchers like [10], prefer to use the term controllability to reachability, 
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a rnullibody car system (a car with n trailers), a mathematical proof of the controllability 
(reachability) was provided in [1]. For nonholonomic systems in general, the reachability 
can be ascertained by constructing the control Lie algebra and then using the controlla- 
bility theorem for nonlinear systems (3). The control Lie algebra is the smallest involutive 
distribution containing the span of the vector fields of the system and closed under Lie 
bracket operations. If the rank of this Lie algebra is full at some configuration C of the 
system, then there exists a neighborhood N of C whose points represent configurations 
reachable by the system from C along admissible paths. Clearly, this condition is a local 
condition. If this condition holds good at every point in the configuration space, then any 
configuration of the system is reachable from any other using admissible paths. 

The controllability (reachability) of a number of simple nonholonomic systems has 
been verified using the approach discussed above. However, for complicated systems like 
a 6DOF free-flying space robot [12], this approach is not useful. In the next section we 
will generate admissible trajectories for simple nonholonomic systems using an algorithm 
based on Stokes’s Theorem. For the two nonholonomic systems that we have considered 
in section 4, we find that reachability can be easily concluded directly from our algorithm. 

An important feature of motion planning algorithms should be their ability to plan 
admissible trajectories amidst additional constraints. In the case of robot manipulators 
these additional constraints may appear in the form of obstacles in the workspace or limits 
imposed on the angular displacement of the joints. In the case of the rolling disk admissible 
trajectories may have to be planned by avoiding obstacles in the x-y plane. Collision-free 
trajectories or trajectories amidst additional constraints have been planned using artificial 
potential functions [7], [13]. In comparison to these approaches, the algorithm discussed in 
this paper has a global attribute. This feature will be evident in the next section through 
examples. 

While discussing the properties of nonholonomic systems in section 2, we realized 
that closed trajectories of the independent variables more often result in a change in the 
dependent variables. This will provide a basis for our motion planning algorithm, where 
closed trajectories of the independent variables will be suitably planned in order to produce 
a desired change in the dependent variables. In particular situations we may however be 
interested in finding closed trajectories of the independent variables that also produce 
closed trajectories of the dependent variables (repeatable motion). Consider the example 
of a planar robot in space with two links mounted in a space vehicle, as shown in Fig.4. In 
this case, the orientation of the space vehicle is the dependent variable while the joint 
variables 0\ and 0 2 are the independent variables. If this robot is expected to perform a 
repeatitive task in space, we would expect all the variables 0 o , 6\, and 0 2 to move along 
closed trajectories. In the next section, we illustrate repeatability in the case of a two-link 
space robot, and for a rolling disk. In the case of the rolling disk repeatable motion leads 
to the generation of singularity free trajectories. 



and adhere to the terminology used in [14]. 
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4. Nonholonomic motion planning using Stokes’s theorem 
4.1 Example 1: A planar space robot 

In this section we illustrate our algorithm for nonholonomic motion planning using 
Stokes’s theorem, through the example of a 2-DOF planar space manipulator. We consider 
a planar free-flying space manipulator consisting of two links mounted on a space vehicle, 
as shown in Fig.4. Such a system can be described by five coordinates: x 0) yo, and 0 O 
representing the position of the center of mass and the orientation of the space vehicle, 
and 0 X and 0 2 representing the joint angles of the manipulator. The variables x 0 and yo can 
be eliminated by using the holonomic constraints due to linear momentum conservation. 
The 2-DOF system is then described by three generalized coordinates 0,, i = 0,1,2, and 
one nonholonomic constraint due to the conservation of angular momentum given by the 
relation 



do — — ^a0i + 

where, 



(13) 



A = mi + m 2 ) 2 l 2 + - (mo + imi)m 2 /d 2 cos0 2 - A/ (^1 + (^mi + m 2 )^ + ^m 2 / 2 ^ 

a = -A - M I 0 

b = M ^/ 2 + ^m 2 / 2 + ^m 2 /i/ 2 cos0 2 ^ - ^m 2 / 2 - ^m 2 (^mi + m 2 )b/ 2 cos0 2 (14) 

and where, m 0 , mi and m 2 are the masses of the space vehicle and the two links, 7 0 , b and 
I 2 are the moments of inertia of the space vehicle and the two links about their center of 
masses, b and / 2 are the length of the two links, and M = (mo+m, +m 2 ) and I - (Io+I\ + h)- 

The configuration of the space manipulator can be described by (0o>^i.^2)- Then 
the path planning problem is to find suitable trajectories for 0i and 0 2 that will change 
the current configuration of the space manipulator to some desired configuration. This 
immediately raises the question pertaining to the reachability of the system - is it possible 
to plan trajectories that will take the space manipulator from any initial configuration to 
any final configuration. The answer to this question is yes, and can be obtained directly 
from our path planning algorithm, discussed below. 

Let the arbitrary initial and desired configurations of the space manipulator be denoted 
by (0 Ol> 0u,0. 2 ,) and (0 O /, 0 lf , 0 2/ ) respectively. We first converge the joint variables 0 U and 0 2l 
(the independent variables) to their desired values 0 x j and 0 2 / respectively. In this process, 
let the orientation of the space vehicle (the dependent variable) drift from 0 O i to some value 
Oqu ■ The task is now to plan a cyclic motion for the joints of the manipulator such that the 
orientation of the space vehicle changes from 0o d to 0o/ while the joint angles come back to 
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their desired configuration. The associated reachability problem can be solved if we can 
show that it is possible to change the orientation 0 0 by an arbitrary amount using cyclic 
motion of the joints, at any joint configuration 0 X and 0 2 . For cyclic motion of the joints, 
the change in 0 O is represented as 

J d0 o = j> + bd0 2 ) (15) 

where, A, a, and b have been defined by Eq.(14), and C in Eq.(15) is a closed curve in the 
0i-02 plane that we will suitably choose. Using Green’s theorem given by Eq.(6), the above 
equation is simplified to 



/--//,[ 



A ( “) 

80 1 A ; S0 2 v A ; 



dO\dO 2 



= M l o 



//.*( 



1 



A 13 cos 0'2 



d0\d0‘2 



(16) 



A = (^rni + m 2 ) 2 i\ + - A/ ^/ + (^ 7n i + rn ‘l)i\ + J rn 2 ^ 2 ^ 

13 = —(t/iq -f ^-m x )m 2 l x l 2 (17) 

where, the expressions for a and b were substituted from Eq.(14), and S is the surface in 
the 0i-02 plane confined within the closed curve C. Let the desired change in 0 O be denoted 
as 0o Then the path planning problem reduces to the proper selection of the area S in the 
0 i -02 plane such that the following equality is satisfied 




1 

A + D cos 0 2 



dOxdO-2 



00 A 

mr k 



(18) 



If we choose a rectangular path in the 0 x -0 2 plane, such that the sides of this rectangle 
are parallel to the 0 X and 0 2 axes, then the above identity reduces to 



(*1 n - 0 U ) 



1 

A + D cos 0 2 U 



1 

A + D cos 02i 



= k 



(19) 



where, 0 U and 0i u denote the lower and upper extremities of 0i in the rectangular path 
while 02 / and 0 2u denote the same for 0 2 . 



The reachability of the system can be proven by showing that there exists a surface 
S such that the equality in Eq.(18) can be satisfied for any arbitrary value of k. The 
initial values of 0i and 0 2 does not necessarily have to lie on the boundary of this surface 
S. This follows from our discussion in section 2.2. We first note that if the identity in 
Eq.(18) can be satisfied for some value of k by traveling along the boundary of the area S 
in the positive direction, then the same identity can be satisfied for -k by simply traveling 
along the boundary in the negative direction. Furthermore, if the identity can be satisfied 
for some value of k by traveling once along the boundary of S , then the identity can be 



11 



satisfied for the value nk, n= 1,2,---, by traveling n times along the boundary in the same 
direction. Clearly, the reachability problem reduces to showing that the identity in Eq.(18) 
can be satisfied for any value of A: e [0, t) , where t is some positive small number. For a 
rectangular area S , this is easy to prove. We can always choose the values of 0 2 i and 0 2u in 
Eq.(19) such that the quantity a defined as 

a.( i ! i 

\A + B cos 02u A + B cos 0 2 t ) 

is not equal to zero. Then it is quite obvious that (0 )u - 0 1( ) can be chosen to be equal to 
k/a such that the identity in Eq.(19) can be satisfied for any value of k e (0, c). 

We now illustrate our path planning algorithm with the help of a simple example. We 
consider a space manipulator (refer to Fig.4) of material alluminum, whose kinematic and 
dynamic parameters are given below in SI units: 

Kinematic and Dynamic parameters 





Mass 


Inertia 


Length 


Vehicle 


27.440 


1.520 




Link-1 


5.380 


0.115 


h = 0.50 


Link-2 


2.640 


0.028 


h = 0.35 



Let the initial configuration of the system be (0q x ,0 u , 0 2i ) = (0.0, 15.0, 15.0) degrees, and the 
final configuration be (0 O /, *1 /• 0 2 /) = (-20.0,45.0,0.0) degrees. We first converge 0! and 0 2 
from their current values to their desired values using the straight line trajectory CM, as 
shown in Figs. 5 and 6. In this process, the orientation of the space vehicle drifts from 
0.0 degrees to 0 O j = -12.87 degrees, as shown in Fig. 6. Therefore, 0 O = (0o/ - 0(w) = -7.13 
degrees, or -0.1244 radians. We now plan a cyclic motion of the joints such that after 
three complete cycles of the joint motion the orientation of the space vehicle changes 
by the desired amount. Then, for each cycle, the required change of orientation would be 
-0.1244/3.0 = -0.0415 radians. Using this value for 0 O , the value of k in Eq.(18) is computed 
to be -7.696 x 10 -4 . With the complete liberty to choose the closed path in the joint space, 
we choose the simple directed path given by the rectangle ABCD in Fig. 5. The closed 
curve is chosen such that the intermediate configuration of the system - point A, lies on 
this closed curve. The surface integral in Eq.(l8) then simply reduces to 



JLAa + B-J (d + Bcos6 2 j A + Bcos0 2 i) 

<20) 

where, A and 7 have to be chosen appropriately. We choose A = 125.0 degrees z.e. 2.181 
radians. Then using the values of A = -89.848 and B = -13.92 from Eq.(17) and the table 
above, we obtain the value of 7 = 0.596 radians or 53.36 degrees. For this choice of A and 7, 
the evolution of all the configuration variables for the path OABCD are shown in Fig.6. 
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We would like to mention a few points at this juncture. For the same set of values of 
7 and A, there are infinite paths that will produce the same change in the orientation 0 O . 
As for example, the directed paths O ABC DA (taken from Fig. 5) and OADMNA in Fig. 7 
will produce the same change in the orientation 0 O . In both these cases the intermediate 
configuration ( 0o<n0i/>02/ ) - point A, lie on the closed curves ADCD and ADMN . This only 
simplifies the motion planning problem but is not a necessary condition, as we have already 
discussed in section 2.2. Area ADMN can be concieved as a translation of the area ADCD 
to the left along BA. In fact, the translation of the area ADCD by any amount along the 
line DA will result in feasible closed trajectories. This follows directly from the expression 
for the surface integral in Eq.(20). We see that the surface integral in Eq.(20) depends 
upon the difference of the values of 0 lf and 0 U , but not individually on 0 if and 0 U . There 
are also infinite other combinations of A and 7 values that we can choose to satisfy Eq.(20). 
And there is even more flexibility when we can suitably choose the number of cycles of 
joint motion that will produce the desired change in the orientation 0 O . As for example, 
in the particular situation discussed above, the directed path AXYZ can bring about the 
same change in 0 O in four cycles of the joint motion that the paths ABCD and ADMN can 
bring about in three cycles. The path AXYZ was obtained by choosing 7 = 75.0 degrees 
which led to the value of A = 76.084 degrees. 

We next consider the practical situation where the trajectory of the system may have 
to be planned subject to the joint limits |0i| < 120 degrees. Then for the above example, the 
trajectories ADMN and AXYZ in Fig. 7 would be feasible whereas the trajectory ADCD 
would not be feasible. It is quite clear that our path planning algorithm provides us with 
the flexibility in choosing trajectories that can satisfy additional constraints, like joint 
limits in the case of space robots. 

We complete this section with a discussion on repeatable motions of the space robot. 
In particular situations, a space robot may be expected to perform a repeatative task 
in space. In such a situation the end-effector of the robot as well as the configuration 
variables of the robot will all have to move along closed trajectories. If the joints of the 
robot, shown in Fig. 4, move along closed trajectories, the dependent variables x 0 and y 0 
will always move along closed trajectories because of the holonomic nature of the linear 
momentum constraints. The dependent variable 0 O will however not move along a closed 
trajectory in the general case. If the net change in 0 O were also to be zero as the joints 
moved along a closed rectangular path, then from Eq.(19), the necessary conditions that 
would have to be satisfied are 0 iu = 0 1( , or cos 0 2u = cos 0 M (assuming a rectangular path in 
the Ox -02 plane). The first condition leads us to the trivial case where the first joint of the 
robot will have to be kept fixed. The second condition tells us that repeatability is assured 
for 0 2u + O 21 = 2n7r, n = 0, ±1, ±2, • • •. In the previous example that we considered, we found 
that we had a significant amount of flexibility in choosing the closed trajectories. Though 
the condition 02 u + On = 2nn, n = 0,±1,±2, •••, will restrict our choices, we should still be 
able to choose from a variety of paths that will produce repeatable motion. 
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4.2 Example 2: A disk rolling without slipping on a fiat surface 

We revisit the classical example of the disk rolling without slipping on a flat surface 
and this time we apply our algorithm for its nonholonomic path planning. The configu- 
ration space of the rolling disk, as discussed in section 1, is described by (x,y,0,a). The 
independent variables of the system are 9 and a, and x and y are the dependent variables 
constrained by the expressions given in Eq.(3). While planning a path from an initial 
configuration of the disk {x t ,y u O t ,a x ) to some desired configuration ( x s ,y s ,0 s ,a s ), we first 
converge the independent variables to their desired values using some simple trajectory 
without being much concerned about the evolution of the dependent variables. This tra- 
jectory may however be important so as to avoid a singularity problem, which we shall 
discuss soon. At the end of this trajectory (path segment OP in Fig. 8), the dependent 
variables are assumed to drift to values x d and y d • Now the task is to plan a closed path 
for the independent variables 0 and a such that the dependent variables x and y change 
their values from x d and y d to x/ and yj respectively. Let C be such a closed path in the 
0-a plane. Then the change of the variables x and y are represented as 



(X/ - x d ) = 


j) r sin adO = J J —r cos a dO da 


(21) 


( y f - yd) = 


(j> r cos adO= J J r sin a dO da 


(22) 



where, we applied Green’s theorem, given by Eq.(6), to convert the line integral into the 
surface integral. Therefore, S is the surface in the 0-a plane within the closed curve C. We 
choose the closed curve C as the directed rectangular path PQRSP as shown in Fig. 8. For 
this rectangular path PQRSP, Eqs.(21) and (22) yield 



(x f — x d ) — —2 a r sin(6/2) cos(a j + 6/2) (23) 

( y j - y d ) = 2arsin(6/2) 510(0^ + 6/2) (24) 

Assuming 6 ^ 2nn, n = ±1, ±2, • • •, we solve for a and 6 as follows 



6 = 2 (—o/ + arctan2 (yj — y d ,x d — x /)] , 0 < 6 < 4n 



(25) 



{(x/ - J<j ) 2 + ( y i - ya ) 2 }' /2 

2 t sin(6/2) 



a > 0 



( 26 ) 



As the disk would move along the sides QR and SP of the rectangular path PQRSP in 
Fig.8, the value of o would have to change in the absence of rolling. This may not be simple 
to achieve in practice, as for example in the case of an unicyclist. We therefore modify 
our rectangular path to the path PQMNP in Fig.8 where o would change only when the 
disk is rolling. It is easy to show that surface the integral in Eq.(21), as in Eq.(22), will 
have the same value when the closed curve C is the rectangle PQRSP or the parallelopiped 
PQMNP. 
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Equation (26) has a singularity for 



ttfcUm2{y f - y d ,x d - x/) = ctj ( 27 ) 

and there are three simple ways to overcome this singularity problem. One way would be 
to arrive from the initial configuration to a different intermediate configuration i.t. at a 
different set of values x d and y d , that would not satisfy Eq.(27). This would require us 
to choose a different trajectory from O to P (see Fig. 8). The second alternative would be 
to set an intermediate goal x' f , y' s and first move to this configuration from x d , y d using 
cyclic motion of the independent variables. Then the remaining task would be to plan a 
second cyclic motion of the independent variables such that the dependent variables would 
converge to x/, yj from their values x' f , y' f . The smart alternative would be to follow the 
three step procedure explained below and diagrammatically shown in Fig.9. 

1 Change the present configuration variables 0/ and a/ to some other values 0) and a) 
using any trajectory OR, such that a) ± a Jt at R. Ignore the evolution of the variables 
x and y that take the new values x' d , y' d at R from their values x d ,y d at O. 

2 Construct any closed path C passing through R, that will change the x and y variables 
by amounts (x f -x d ) and ( y s - y d ) respectively. If we choose a rectangular path, or the 
equivalent parallelogram path, then the dimensions of this path would be computed 
from Eqs.(24) and (25) only by replacing a f in Eq (24) by a). Move along this closed 
trajectory once to come back to R where 0 = 0) and a = a). 

3 Retrace backwards the trajectory OR to move from R to O At O the configuration of 
the system would be ( x f ,yj,0j,af ). 

The above procedure for singularity avoidance follows from our discussion in section 
2.2. This procedure is also recommended for avoiding points close to singular points. 
At points close to the singularity, trajectories tend to become infeasible due to the large 
magnitude of a, as evident from Eq.(26). 

We are now convinced that any configuration of the rolling disk ( xj,yj,0j,aj ) can be 
reachable from any initial configuration (x,, y,, 0,, a,) by using the motion planning algorithm 
discussed in this section. Though in certain situations, there may be a singularity problem, 
this problem can be easily overcome. The singularity that we may encounter is however 
not a physical singularity, it is rather an algorithmic singularity. The singularity problem 
can be completely removed by adopting a slightly different algorithm. This algorithm, to 
be discussed shortly, uses the repeatable motion of one of the dependent variables. This 
algorithm will further strengthen our conviction on the reachability issue of the rolling 
disk. 

We illustrate our path planning algorithm with the help of a simple example. In this 
example we come close to the singularity and we tactfully avoid it using the algorithm 
discussed above. Consider a disk of radius r — 0.25 m which is at its current configuration 
(x,, y,, 0,, o,) = (0.0, 0.0, 0.0, 0.0) metres, degrees. Suppose the desired configuration of this 
disk is [xj,y s ,0j,cxj) = (- 0 . 4 , 1 . 0 , 180 . 0 , 22 . 5 ) metres, degrees. We first converge the vari- 
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ables 0 and a to their desired values using the simple straight line path OZ, as shown in 
Fig. 10. The coordinates of x and y at the end of this path will be 0.1522 and 0.7654 metres 
respectively (obtained through numerical simulation). Using Eqs.(25) and (26), we solve 
for a and b as 6 = 0.01808 and a = 132.72 radians respectively. Clearly, we are close to a 
singular configuration. In the light of the discussion on singularity avoidance, we adopt 
the following measure. We have the complete liberty to choose 0' f and a’ f (point R in 
Fig. 9), with the only restriction that a) / a f . We choose (0), a' f ) = (0,,a,) = (0.0, 0.0). And 
we change 0 y and ay to 0, and a, by retracing the path OZ backwards (see Fig. 10). We 
therefore come back to the initial configuration where ( x,y,6,a ) = (x,, y„ 0,, a,). Now we 
substitute the value of a, in place of ay in Eq.(25) and solve for a and b from Eqs.(25) 
and (26). We obtain b = 0.8034 radians or 46.03 degrees and a = 3.068 radians or 175.78 
degrees. In Fig. 10, the path OPQRO is the closed path constructed with these values of a 
and b. Due to the motion along this closed path the change in the x and y coordinates will 
be (xy - x d ) = (-0.4 - 0.1522) = -0.5522 metre and ( yj - y d ) = (1.0 - 0.7654) = 0.2346 metre 
respectively. Therefore the coordinates at O of the dependent variables after the motion 
along the closed path will be x = -0.5522 and y = 0.2346 metres. We finally trace the 
straight line path from O to Z. Due to this motion the change in the x and y coordinates 
will be (x d - x.) = (0.1522 - 0.0) = 0.1522 metre and ( y d - y t ) = (0.7654 - 0.0) = 0.7654 metre 
respectively. Therefore, the coordinates at Z will be (-0.5522 + 0.1522) = -0.4 metre and 
(0.2346+0.7654) = 1.0 metre respectively. The coordinates of the independent variables at Z 
are obviously 0 = 180.0 degrees and a = 22.5 degrees. Looking back at the entire motion, we 
realize that the initial path from O to Z and back to O is redundant. Therefore, the path 
that will be sufficient for converging all the variables will be OPQROZ, as shown in Fig. 10. 
The closed path OPQRO is chosen to be a parallelogram instead of a rectangle for reasons 
we have already discussed earlier in this section. The complete path OPQROZ is quite 
different from paths that are generated in the absence of singularity. In the absence of 
singularity, the complete path consists of an initial path segment followed by a closed loop 
in the 6-a plane. In the particular example that we have considered, we had a singularity 
and the complete path consisted of a closed loop path followed by a simple path segment. 
The simulation results of this particular example have been shown in Fig. 11. The points 
O, P, Q, R, and Z in Fig. 11 correspond to the same points in Fig. 10. 

The singularity problem discussed above arises due to the particular nature of our 
algorithm where we converge both the dependent variables x and y simultaneously using 
closed trajectories of the independent variables. We have seen that this is not at all 
a serious problem. However, this problem can be completely eliminated by adopting a 
slightly different algorithm. The idea behind this algorithm is to use repeatable motion of 
one of the dependent variables. This algorithm is discussed next. 

In our singularity free algorithm, we will first converge the independent variables 0 
and q from their initial values (0,,a,) to their desired values (0y,ay). Let us suppose that 
the dependent variables x and y change their coordinates from x, and y, to x d and y d 
respectively. We will next converge x to its desired value xy using closed trajectories of the 
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independent variables without being concerned about the evolution of the y coordinate. 
Specifically, we will use Eq.(23), where we will have the liberty to choose a set of values a 
and b. Let us suppose that the dependent variable y drift from its previous coordinate y d 
to y' d in this process. We will finally converge y from its present coordinate y' d to its desired 
value yj using closed trajectories of the independent variables that will also produce a 
closed trajectory of the dependent variable x, i.e. a repeatable motion in the x coordinate. 
Prom Eqs.(23) and (24) it follows that the correct choice of the variables a and b for this 
repeatable motion should be 



b = — 2af ± mr, 



a 



Ay 

2 r cos a j 1 



cij 7 ^ ±( 2 n+ 1 )7t/2. 



n = 0,1,2,- •• (28) 



where A y = (y/ -y' d ), and b will be chosen such that b > 0. The magnitude of a can however 
be positive or negative. We will use the absolute value of a to construct the parallelogram 
path in the 0-a plane. If the sign of a is obtained negative from Eq.(28), then the use of 
the positive value will bring about a change in the y coordinate by an amount -Ay instead 
of Ay. This problem can be simply solved by changing the direction of travel along the 
closed path. This idea has been appropriately demonstrated in the next simulation. 

As an alternative, we could also converge the dependent variable y before we converge 
the dependent variable x. In that case, after the initial motion from the configuration 
(x,,y„0,,a,) to the configuration ( x d ,y d ,0f,a/ ), we would suitably choose a and b in Eq.(24) 
such that y converges from y d to yj. At this stage we will not be concerned about the 
evolution of the x coordinate, which will probably drift from x d to x' d . Finally, we will 
change the variable x by an amount Ax = (x; - x' d ) using closed trajectories of 0 and a 
that will also produce a closed trajectory of y, i.e. a repeatable motion in the y coordinate. 
From Eqs.(23) and (24) it then follows that the correct choice of the variables a and b 
would be 



b = 2 (n 7r — a/), a ~o : . ct f ^±mr, n = 0, 1,2,-- (29) 

i r sm a / 

where, b will be chosen such that it is a positive number, and the absolute value of a will 
be used to construct the trajectory. The direction of travel along the closed trajectory 
should be along the positive or the negative direction depending upon whether the sign of 
a comes out to be positive or negative in Eq.(29). 

From the two alternatives we conclude that if the final value of a is such that a s = 
±(2n+ 1 )7r/2, for n = 0, 1,2, •••, then we will first converge y and then converge x. If 
q / = ± n u, for n = 0, 1,2, •••, then we will first converge x and then converge y. For 
a/ ^ n-n/2, for n = 0, 1,2, • either of the two alternatives mentioned above can be adopted. 

To illustrate the efficacy of this singularity free algorithm, we consider the same ex- 
ample we have considered before. The initial configuration of the disk of radius r = 0.25 
metres is ( L x l ,y l ,8 t ,a t ) = (0.0, 0.0, 0.0, 0.0) metres, degrees, and its desired configuration is 
( xf,y f ,0f,uf ) = (-0.4,1.0,180.0,22.5) metres, degrees. We first converge the independent 
variables using a straight line path (path segment OP in Figs. 12 and 13) in the 0-a 
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plane. At the end of this path the configuration of the system is found to be ( x,y,6,a ) = 
( 0 . 1522 , 0 . 7654 , 180 . 0 , 22 . 5 ) metres and degrees respectively. Since a f = 22.5 degrees ^ rnr/2, 
for n = 0, 1,2, • • •, therefore we can choose to first converge x or y. We choose to converge x 
first. In Eq.(23), we substitute x f = - 0.4 metres, x d = 0.1522 metres, and a f = 22.5 degrees. 
Choosing 6 = 60 degrees or 1.047 radians, we obtain a = 3.628 radians or 207.87 degrees. For 
these values of a and b, the closed path is given by PQRSP in Fig. 12. After moving along 
this path, the y variable drifts from y d = 0.7654 metres to y' d = 1.485 metres, as shown in 
Fig. 13, whereas the x coordinate converges to its desired value. Now the task is to generate 
a closed path in the 0-a plane that will cause a repeatable motion in x but will converge y 
to yj — 1.0 metre. Using Eq.(28) we arrive at b = 135 degrees, or 2.356 radians and a — - 1.05 
radians, or 60.15 degrees. For these particular values of a and b, the closed trajectory is 
given by PXYZP in Fig. 12. Due to the negative value of a the direction of this closed 
path is opposite to our usual convention. The entire motion of the system can be obtained 
by eliminating the redundant path segment SPS. Therefore the complete motion of the 
system would be OPQRSXY ZP, as shown in both Figs. 12 and 13. 

5. Conclusion 

A motion planning algorithm for nonholonomic mechanical systems was presented in 
this paper. In this algorithm, the independent variables of the system were first converged 
to their desired values. Subsequently, the dependent variables were converged using closed 
trajectories of the independent variables. The task of motion planning was simplified using 
Stokes’s theorem. This reduced the task of finding closed trajectories of the independent 
variables into that of finding a surface area in the space of the independent variables such 
that the dependent variables converged to their desired values while the independent vari- 
ables traversed along the boundary of this surface area. The motion planning algorithm 
was found to have certain global attributes due to which questions pertaining to the reach- 
ability of the system could be easily answered and the motion could be planned amidst 
additional constraints. The salient features of the algorithm was aptly illustrated through 
the examples of a planar space robot and a disk rolling without slipping on a flat surface. 
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Figure 1 . A disk rolling on a Hat surface is described by four configuration 
variables: x, y, 0, a. The disk has however two degrees of freedom due to 
the presence of two nonholonomic constraints. 




Figure 2 (a). Positive direction of travel along the closed curve C in Stokes Theorem 




Figure 2 (b). Positive direction of travel along the closed curve C in Green's Theorem 
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Figure 3 (a). The closed trajectory C in the independent variables x, y, and z 
produces a change in the dependent variable p by an amount Ap. The initial 
configuration of the system - (1), lies on this closed trajectory. 




Figure 3 (b). The closed trajectory C in the independent variables x, y, and z 
produces a change in the dependent variable p by an amount Ap. The initial 
configuration of the system - (1), does not lie on this closed trajectory. 
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Figure 4. A two-link manipulator mounted on a space vehicle is described 
by three generalized coordinates: 0 O , 0, , 0 2 . The center of mass of the 
space vehicle has the coordinates x 0 , y 0 . 
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Figure 5. The path segment OA followed by the closed path ABCDA was 
used to converge all the configuration variables of the space robot, for the 
simulation discussed in section 4.1. The closed path was repeated 3 times. 




Figure 6. Evolution of all the configuration variables of the space robot 
with time, for the simulation discussed in section 4.1. Points O, A, B, C 
and D in this figure correspond to the same points in Fig.5. 




Figure 7. Directed paths ABCD, and ADMN both change the orientation of the 
space vehicle by equal amounts. The closed path AXYZ changes the orientation 
of the vehicle in four cycles by the same amount that the closed paths ABCD and 
ADMN can bring about in three cycles. 
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0= (Xj.yi.ej.ctj) 
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(before executing loop) 

P= (x f ,yf,e f ,a f ) 
(after executing loop) 



Figure 8. Both the closed paths PQRSP and PQMNP in the 0-a plane 
produce the same desired changes in the dependent variables x and y. 
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Figure 9. A diagrammatic representation of the singularity avoidance scheme 
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Figure 10. The path OPQROZ is the outcome of the successful implementation 
of the singularity avoidance scheme for the rolling disk, discussed in section 4.2. 
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Figure 11. Path OPQROZ shows the motion of the disk rolling on the 
x-y plane, for the simulation discussed in section 4.2 on the singularity 
avoidance scheme. Fig. 10 shows the motion of the disk on the 0— a plane. 
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Figure 12. The path OPQRSXYZP was planned using the singularity 
free algorithm with a repeatable motion in the x coordinate 
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Figure 13. Path OPQRSXYZP shows the motion of the disk rolling on the 
x-y plane. This motion was planned using the singularity-free algorithm, 
discussed in section 4.2, with a repeatable motion in the x coordinate. The 
corresponding motion of the diskin the 0-a planeisshowninFig.il. 
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